﻿using System;
using System.Collections.Generic;
using System.Text;

namespace Gert3D
{
  /// <summary>
  /// Used to store plucker coordinates of a line in 3-space.
  /// </summary>
  struct Plucker
  {
    public double P0;
    public double P1;
    public double P2;
    public double P3;
    public double P4;
    public double P5;

    public Plucker(double p0, double p1, double p2, double p3, double p4, double p5)
    {
      P0 = p0;
      P1 = p1;
      P2 = p2;
      P3 = p3;
      P4 = p4;
      P5 = p5;
    }
  }

  /// <summary>
  /// Ray is a ray that starts at an origin and points into direction.
  /// It can be used to test for intersections with spheres and triangles.
  /// </summary>
  public class Ray
  {
    /// <summary>
    /// The internal field for the origin of the ray.
    /// </summary>
    protected Vector3 _origin = new Vector3();

    /// <summary>
    /// The internal field for the direction of the ray.
    /// </summary>
    protected Vector3 _direction = new Vector3();

    /// <summary>
    /// The internal field for the length of the ray.
    /// </summary>
    protected double _length = 1.0f;

    /// <summary>
    /// The internal field for the Zero property.
    /// </summary>
    protected static Ray _zero = new Ray();

    /// <summary>
    /// Creates a new ray at origin (0,0) in direction (0,0) with a lenght of 0.
    /// </summary>
    public Ray()
    {
    }

    /// <summary>
    /// Creates a new ray starting at the given origin and pointing into the given direction.
    /// The length is set to 1.
    /// </summary>
    /// <param name="origin">The origin of the new ray.</param>
    /// <param name="direction">The direction of the new ray.</param>
    public Ray(Vector3 origin, Vector3 direction)
    {
      _origin = origin;
      _direction = direction;
    }

    /// <summary>
    /// Creates a new ray starting at the given origin, pointing into the given direction and with the given length.
    /// </summary>
    /// <param name="origin">The origin of the new ray.</param>
    /// <param name="direction">The direction of the new ray.</param>
    /// <param name="length">The length of the new ray.</param>
    public Ray(Vector3 origin, Vector3 direction, double length)
    {
      _origin = origin;
      _direction = direction;
      _length = length;
    }

    /// <summary>
    /// Create a new ray that start at start and pointing to end.
    /// </summary>
    /// <param name="start">The start of the ray.</param>
    /// <param name="end">The point the ray points to.</param>
    public static Ray FromPoints(Vector3 start, Vector3 end)
    {
      Ray result = new Ray();
      result._origin = start;
      result._direction = Vector3.FromPoints(start, end);
      result._length = result._direction.Magnitude;
      result._direction.Normalize();
      return result;
    }

    /// <summary>
    /// Gets or sets the origin of the ray.
    /// </summary>
    public Vector3 Origin
    {
      get { return _origin; }
      set { _origin = value; }
    }

    /// <summary>
    /// Gets or sets the direction of the ray.
    /// </summary>
    public Vector3 Direction
    {
      get { return _direction; }
      set { _direction = value; }
    }

    /// <summary>
    /// Gets or sets the length of the ray.
    /// </summary>
    public double Length
    {
      get { return _length; }
      set { _length = value; }
    }

    /// <summary>
    /// Gets a ray at origin (0,0), direction (0,0) and length 0.
    /// </summary>
    public static Ray Zero
    {
      get { return _zero; }
    }

    /// <summary>
    /// Calculates the plucker coeficients of the line that is expressed by the two given points.
    /// </summary>
    /// <param name="p">The start point of the line.</param>
    /// <param name="q">The end point of the line.</param>
    /// <returns>The calculated plucker coeficients.</returns>
    private Plucker PluckerCoords(Vector3 p, Vector3 q)
    {
      return new Plucker(p.X * q.Y - q.X * p.Y, p.X * q.Z - q.X * p.Z, p.X - q.X, p.Y * q.Z - q.Y * p.Z, p.Z - q.Z, q.Y - p.Y);
    }

    /// <summary>
    /// Calculates the side product (also called permuted inner product) of the two given plucker line coeficients.
    /// The returned value is proportional to the signed volumes of the tetrahedron that is formed by the start and end
    /// points (4 points) of the lines.
    /// </summary>
    /// <param name="a">The plucker coeficients of the first line.</param>
    /// <param name="b">The plucker coeficients of the second line.</param>
    /// <returns></returns>
    private double side(Plucker a, Plucker b)
    {
      return a.P0 * b.P4 + a.P1 * b.P5 + a.P2 * b.P3 + a.P3 * b.P2 + a.P4 * b.P0 + a.P5 * b.P1;
    }

    /// <summary>
    /// Calculates the intersection point of the ray and the given triangle.
    /// For additional optimization the minT parameter is passed. It specifies the
    /// t value of the closest intersection that has happened so far. This is usefull
    /// when checking for intersection of multiple triangles, because the algorithm
    /// exits early if the calculated t is bigger than the given minT.
    /// </summary>
    /// <param name="tri">The triangle to check for intersection.</param>
    /// <param name="minT">The t value of the closest intersection so far.</param>
    /// <returns>The intersection information or null if there is no intersection.</returns>
    public IntersectionInfo IntersectTriangle(Triangle tri, double minT)
    {
      //Used to test values for beeing close to zero because of limited precision
      double eps = 0.0000001;
      double neps = -0.0000001;

      Vector3 ta = tri.VertexA.Position;
      Vector3 tb = tri.VertexB.Position;
      Vector3 tc = tri.VertexC.Position;

      Vector3 rayEnd = new Vector3(
        _origin.X + _direction.X * _length,
        _origin.Y + _direction.Y * _length,
        _origin.Z + _direction.Z * _length);

      Plucker tab = PluckerCoords(ta, tb);

      //Calculate t of intersection of ray with triangle plane
      //WARNING: Somehow this is not 100% correct. Don't use for further calculation
      double g1 = side(tab, PluckerCoords(tc, _origin));
      double g2 = side(tab, PluckerCoords(tc, rayEnd));
      double t = g1 / (-g2);

      // If there already was an intersection closer to the camera then return null
      if (t < eps || t > minT)
        return null;

      //Calculate barycentric coordinates
      Plucker rayPluck = PluckerCoords(_origin, rayEnd);
      double s1 = side(rayPluck, tab);
      double s2 = side(rayPluck, PluckerCoords(tb, tc));
      double s3 = side(rayPluck, PluckerCoords(tc, ta));

      //if ((s1 >= eps && s2 >= eps && s3 >= eps) || (s1 <= neps && s2 <= neps && s3 <= neps))
      if ((s1 > neps && s2 > neps && s3 > neps) || (s1 < eps && s2 < eps && s3 < eps))
      {
        //Side products are proportional to the signed area
        //of the barycentric triangles. So scale them to sum up to 1 to get barycentric coordinates.
        double sum = s1 + s2 + s3;
        s1 /= sum; //Barycentric C
        s2 /= sum; //Barycentric A
        s3 /= sum; //Barycentric B

        IntersectionInfo result = new IntersectionInfo();

        // Calculate using barycentric coordinated because t value is not correct. This is even faster!
        result.IntersectionPoint = new Vector3(
          ta.X * s2 + tb.X * s3 + tc.X * s1,
          ta.Y * s2 + tb.Y * s3 + tc.Y * s1,
          ta.Z * s2 + tb.Z * s3 + tc.Z * s1);

        //Interpolate normal
        result.Normal = new Vector3(
          tri.VertexA.Normal.X * s2 + tri.VertexB.Normal.X * s3 + tri.VertexC.Normal.X * s1,
          tri.VertexA.Normal.Y * s2 + tri.VertexB.Normal.Y * s3 + tri.VertexC.Normal.Y * s1,
          tri.VertexA.Normal.Z * s2 + tri.VertexB.Normal.Z * s3 + tri.VertexC.Normal.Z * s1);

        //Interpolate texture coordinates
        result.TexCoords = new Vector3(
          tri.VertexA.TextureCoordinates.X * s2 + tri.VertexB.TextureCoordinates.X * s3 + tri.VertexC.TextureCoordinates.X * s1,
          tri.VertexA.TextureCoordinates.Y * s2 + tri.VertexB.TextureCoordinates.Y * s3 + tri.VertexC.TextureCoordinates.Y * s1,
          tri.VertexA.TextureCoordinates.Z * s2 + tri.VertexB.TextureCoordinates.Z * s3 + tri.VertexC.TextureCoordinates.Z * s1);

        //Fill other intersection info
        result.Barycentric = new Vector3(s2, s3, s1);
        result.T = t;
        
        return result;
      }
      else
        return null;
    }

    /// <summary>
    /// Calculates the point of intersection of the ray with the given sphere.
    /// For additional optimization the minT parameter is passed. It specifies the
    /// t value of the closest intersection that has happened so far. This is usefull
    /// when checking for intersection of multiple spheres, because the algorithm
    /// exits early if the calculated t is bigger than the given minT.
    /// </summary>
    /// <param name="sphere">The sphere to check for intersection.</param>
    /// <param name="minT">The t value of the closest intersection so far.</param>
    /// <returns>The intersection information or null if there is no intersection.</returns>
    public IntersectionInfo IntersectSphere(Sphere sphere, double minT)
    {
      double a = _direction._x * _direction._x + _direction._y * _direction._y + _direction._z * _direction._z;

      double relX = _origin.X - sphere.Center.X;
      double relY = _origin.Y - sphere.Center.Y;
      double relZ = _origin.Z - sphere.Center.Z;
      double b = 2 * (_direction._x * (relX) + _direction._y * (relY) + _direction._z * (relZ));
      double c = Math.Pow(relX, 2) + Math.Pow(relY, 2) + Math.Pow(relZ, 2) - Math.Pow(sphere.Radius, 2);

      double discriminant = (b * b) - (4 * a * c);
      if (discriminant < 0)
        return null;

      double t = (-b - Math.Pow(b * b - 4 * c, 0.5)) / 2f;
      if (t < 0 || t < minT)
        return null;

      IntersectionInfo result = new IntersectionInfo();
      result.IntersectionPoint = new Vector3(_origin.X + _direction._x * t, _origin.Y + _direction._y * t, _origin.Z + _direction._z * t);
      result.Normal = Vector3.FromPoints(sphere.Center, result.IntersectionPoint);
      result.Normal.Normalize();
      result.TexCoords = new Vector3();
      result.T = t;

      return result;
    }

    public IntersectionInfo IntersectBox(Box3 box, double minT)
    {
      Vector3 rayDelta = new Vector3(
        _direction.X * _length,
        _direction.Y * _length,
        _direction.Z * _length);

      bool inside = true;

      double xt, xn = 0.0;
      if (_origin.X < box.Left)
      {
        xt = box.Left - _origin.X;
        if (xt > rayDelta.X)
          return null;
        xt /= rayDelta.X;
        inside = false;
        xn = -1.0;
      }
      else if (_origin.X > box.Right)
      {
        xt = box.Right - _origin.X;
        if (xt < rayDelta.X)
          return null;
        xt /= rayDelta.X;
        inside = false;
        xn = 1.0;
      }
      else
      {
        xt = -1.0;
      }

      double yt, yn = 0.0;
      if (_origin.Y < box.Bottom)
      {
        yt = box.Bottom - _origin.Y;
        if (yt > rayDelta.Y)
          return null;
        yt /= rayDelta.Y;
        inside = false;
        yn = -1.0;
      }
      else if (_origin.Y > box.Top)
      {
        yt = box.Top - _origin.Y;
        if (yt < rayDelta.Y)
          return null;
        yt /= rayDelta.Y;
        inside = false;
        yn = 1.0;
      }
      else
      {
        yt = -1.0;
      }

      double zt, zn = 0.0;
      if (_origin.Z < box.Front)
      {
        zt = box.Front - _origin.Z;
        if (zt > rayDelta.Z)
          return null;
        zt /= rayDelta.Z;
        inside = false;
        zn = -1.0;
      }
      else if (_origin.Z > box.Back)
      {
        zt = box.Back - _origin.Z;
        if (zt < rayDelta.Z)
          return null;
        zt /= rayDelta.Z;
        inside = false;
        zn = 1.0;
      }
      else
      {
        zt = -1.0;
      }

      IntersectionInfo result = new IntersectionInfo();
      if (inside)
        return null;

      int which = 0;
      double t = xt;
      if (yt > t)
      {
        which = 1;
        t = yt;
      }
      if (zt > t)
      {
        which = 2;
        t = zt;
      }

      //Return null if there already was an intersection closer to the ray origin
      if (t > minT)
        return null;

      double x, y, z;
      switch (which)
      {
        case 0:
          y = _origin.Y + rayDelta.Y * t;
          if (y < box.Bottom || y > box.Top)
            return null;
          z = _origin.Z + rayDelta.Z * t;
          if (z < box.Front || z > box.Back)
            return null;
          x = _origin.X + rayDelta.X * t;

          result.Normal = new Vector3(xn, 0.0, 0.0);
          break;

        case 1:
          x = _origin.X + rayDelta.X * t;
          if (x < box.Left || x > box.Right)
            return null;
          z = _origin.Z + rayDelta.Z * t;
          if (z < box.Front || z > box.Back)
            return null;
          y = _origin.Y + rayDelta.Y * t;

          result.Normal = new Vector3(0.0, yn, 0.0);
          break;

        case 2:
          x = _origin.X + rayDelta.X * t;
          if (x < box.Left || x > box.Right)
            return null;
          y = _origin.Y + rayDelta.Y * t;
          if (y < box.Bottom || y > box.Top)
            return null;
          z = _origin.Z + rayDelta.Z * t;

          result.Normal = new Vector3(0.0, 0.0, zn);
          break;

        default:
          throw new Exception("Whooops!");
      }

      result.IntersectionPoint = new Vector3(x, y, z);
      result.T = t;

      return result;
    }

  }

}
